/********** System Includes ***************************************************/

#include <iostream>

/********** Local Includes ****************************************************/

#include "blobdetector.h"

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Local Static Function Prototypes **********************************/

bool blobCompare(const BlobDetector::Blob &one, const BlobDetector::Blob &two);
double euclidean(Point p1, Point p2);

/********** Public Methods ****************************************************/

BlobDetector::BlobDetector(const ColorThresholds &red, int redBlur,
	const ColorThresholds &green, int greenBlur, const ColorThresholds &blue, 
	int blueBlur, int cameraCode) :
	m_cameraCode(cameraCode)
{
	m_thresholds[COLOR_RED] = red;
	m_thresholds[COLOR_GREEN] = green;
	m_thresholds[COLOR_BLUE] = blue;
	
	m_blurSizes[COLOR_RED] = redBlur;
	m_blurSizes[COLOR_GREEN] = greenBlur;
	m_blurSizes[COLOR_BLUE] = blueBlur;
	
	SimpleBlobDetector::Params params;
	params.minThreshold = 40;
	params.maxThreshold = 60;
	params.thresholdStep = 5;
	params.minArea = 50; 
	params.minConvexity = 0;
	params.minInertiaRatio = 0;
	params.maxArea = 60000;
	params.maxConvexity = 255;
	params.filterByColor = false;
	params.filterByCircularity = false;
	
	m_sbd = SimpleBlobDetector(params);
	m_sbd.create("SimpleBlob");
}

vector<BlobDetector::Blob> BlobDetector::findBlobs(const string &filename) {
	vector<Blob> blobs;
	
	/*if (filename.size() == 0) {
		m_camera >> m_image;
	} else {
		m_image = imread(filename, 1);
	}*/
	
	/*FILE * f = popen( "ls img", "r" );
	
    if ( f == 0 ) {
        fprintf( stderr, "Could not execute\n" );
        return vector<Blob>();
    }
    
    const int BUFSIZE = 1000;
    char buf[ BUFSIZE ];
    int i;
    for(i = 0; fgets( buf, BUFSIZE,  f ); i++) {}
    pclose( f );
    
    cout << "Using image " << i - 2 << endl;
    
    stringstream ss;
    ss << "img/img_";
    ss << i - 2;
    ss << ".jpg";
    
    m_image = imread(ss.str(), 1);*/
	
	if (m_image.empty()) {
		cerr << "WARNING in " << __FUNCTION__ << " on line " << __LINE__
			<< ": no image to work with" << endl;
		return vector<Blob>();
	}
	
	// Iterates over all of the elements in m_thresholds
	// it->first is the key (color), it->second is the value (thresholds struct)
	for (map<Color, ColorThresholds>::const_iterator it = m_thresholds.begin();
		it != m_thresholds.end(); ++it)
	{
		Mat blurred;
		blur(m_image, blurred, Size(m_blurSizes[it->first],
			m_blurSizes[it->first]));
		
		std::vector<Blob> result = findBlobs(blurred, it->first, it->second);
		
		// Adds all of the elements in result to blobs
		blobs.insert(blobs.end(), result.begin(), result.end());
	}
	
	return blobs;
}

vector<BlobDetector::Blob> BlobDetector::filterForLargest(
	const vector<BlobDetector::Blob> &allBlobs) const
{
	map<Color, int> max; // The maximum number of blobs to accept for each color
	map<Color, int> accepted; // The number already accepted
	
	for (map<Color, ColorThresholds>::const_iterator it = m_thresholds.begin();
		it != m_thresholds.end(); ++it)
	{
		// By default, only accept one of each color
		max[it->first] = 1;
		accepted[it->first] = 0;
	}
	
	// We'll sort the blobs by size, then take as many of each color as we can
	vector<Blob> copy(allBlobs);
	sort(copy.begin(), copy.end(), blobCompare);
	
	vector<Blob> largest;
	
	for (vector<Blob>::const_iterator it = copy.begin(); it != copy.end(); ++it) {
		map<Color, int>::const_iterator iMax = max.find(it->color);
		map<Color, int>::const_iterator iAccepted = accepted.find(it->color);
		
		if (iMax == max.end() || iAccepted == accepted.end()) {
			cerr << "WARNING in " << __FUNCTION__ << " on line " << __LINE__
				<< ": allBlobs contains a blob with an unrecognized color"
				<< endl;
			continue;
		}
		
		// If we have not yet reached the quota for this color...
		if (iMax->second > iAccepted->second) {
			largest.push_back(*it);
			accepted[it->color]++;
		}
	}
	
	return largest;
}

map<BlobDetector::BlobCategory, BlobDetector::Blob> BlobDetector::categorize(
	const vector<BlobDetector::Blob> &largest) const
{
	map<BlobCategory, Blob> cat;
	
	for (vector<Blob>::const_iterator it = largest.begin();
		it != largest.end(); ++it)
	{
		if (it->color == COLOR_GREEN && cat.find(ROBOT_RIGHT) == cat.end()) {
			cat[ROBOT_RIGHT] = *it;
			cat[ROBOT_RIGHT].category = ROBOT_RIGHT;
		} else if (it->color == COLOR_BLUE && cat.find(ROBOT_LEFT) == cat.end()) {
			cat[ROBOT_LEFT] = *it;
			cat[ROBOT_LEFT].category = ROBOT_LEFT;
		} else if (it->color == COLOR_RED && cat.find(FRUIT) == cat.end()) {
			cat[FRUIT] = *it;
			cat[FRUIT].category = FRUIT;
		}
	}
	
	return cat;
}

void BlobDetector::showDebug(
	const std::map<BlobDetector::BlobCategory, BlobDetector::Blob> &blobs) const
{
	Mat copy;
	m_image.copyTo(copy);
	
	for (map<BlobCategory, Blob>::const_iterator it = blobs.begin();
		it != blobs.end(); ++it)
	{
		std::stringstream ss;
		std::string s;
		ss << it->first;
		s = ss.str();
		
		circle(copy, it->second.center, 20, cvScalar(255,0,0), 10);
		putText(copy, s, Point(it->second.center.x, it->second.center.y + 60),
			FONT_HERSHEY_SIMPLEX, 1, cvScalar(0, 0, 0), 3);
	}
	
	imshow("Categorized Blobs", copy);
}

/********** Private Methods ***************************************************/

void BlobDetector::applyThresholds(const Mat &src, Mat &dst,
	const ColorThresholds &param) const
{
	// slices matrcies that hold H,S and V
	vector<Mat> slices;
	Mat hue;			// hue channel
	Mat hue1;			// Hue upper bound
	Mat hue2;			// Hue lower bound
	Mat hue3;			// hue color filtering
	Mat sat;			// Sat channel
	Mat sat1;			// Sat upper bound
	Mat sat2;			// sat lower bound
	Mat sat3;			// sat color filtering
	Mat val;			// Val channel
	Mat val1;			// Val upper bound
	Mat val2;			// Val lower bound
	Mat val3;			// Val color filtering
	
	// Process Image
	cvtColor (src, dst, CV_BGR2HSV);  // convert raw image to hsv
	
	split(dst,slices); // split image to H,S and V images
	slices[0].copyTo(hue); // get the hue channel
	slices[1].copyTo(sat);
	slices[2].copyTo(val);
	
	// apply thresshold for hue channel
	threshold (hue,hue1,param.hueL,255, CV_THRESH_BINARY); // get lower bound
	threshold (hue,hue2,param.hueH,255, CV_THRESH_BINARY_INV); // get upper bound
	hue3 = hue1 & hue2; // multiply to get color range
	
	// apply thresshold for Sat channel
	threshold (sat,sat1,param.satL,255, CV_THRESH_BINARY); // get lower bound
	threshold (sat, sat2,param.satH,255, CV_THRESH_BINARY_INV); // get upper bound
	sat3 = sat1 & sat2; // multiply 2 matrix to get the sat range
	
	// apply thresshold for Val channel
	threshold (val,val1,param.valL,255, CV_THRESH_BINARY); // get lower bound
	threshold (val, val2,param.valH,255, CV_THRESH_BINARY_INV); // get upper bound
	val3 = val1 & val2; // multiply 2 matrix to get the val range
		
	dst = hue3 & sat3 & val3; // Merge arrays for final thresholded image
}

vector<BlobDetector::Blob> BlobDetector::findBlobs(const Mat& rgbImage,
	Color color, const ColorThresholds &thresholds)
{
	std::vector<Blob> blobs;

	Mat hsvImage;
	// TODO What if the green thresholds aren't set?
	applyThresholds(rgbImage, hsvImage, thresholds);
	
	vector<KeyPoint> sbdResult;
	m_sbd.detect(hsvImage, sbdResult);
	
	
	// Iterates over all of the elements in sbdResult
	for (vector<KeyPoint>::const_iterator it = sbdResult.begin();
		it != sbdResult.end(); ++it)
	{
		Blob blob;
		blob.center = it->pt;
		blob.size = it->size;
		blob.color = color;
		blob.category = UNCATEGORIZED;
		
		blobs.push_back(blob);
	}
	
	return blobs;
}

/********** Local Static Functions ********************************************/

bool blobCompare(const BlobDetector::Blob &one, const BlobDetector::Blob &two)
{
	return one.size >= two.size;
}

// Take the euclidean of the difference between p1 and p2
double euclidean(Point p1, Point p2)
{ 
	return sqrt(pow((double) p2.x - p1.x, 2) + pow((double) p2.y - p1.y, 2));
}

/********** Program Entry *****************************************************/

/*int main(int, char **) {
	BlobDetector::ColorThresholds red, green;
	green.hueL = 60;
	green.hueH = 80;
	green.satL = 100;
	green.satH = 255;
	green.valL = 100;
	green.valH = 255;
	red.hueL = 170;
	red.hueH = 245;
	red.satL = 40;
	red.satH = 255;
	red.valL = 40;
	red.valH = 255;
	
	BlobDetector bd(red, green, 0);
	
	while (true) {
		vector<BlobDetector::Blob> blobs = bd.findBlobs("/Users/john/Desktop/logitech 1.jpg");
		cout << "Found " << blobs.size() << " blobs" << endl;
		
		blobs = bd.filterForLargest(blobs);
		cout << "The largest are: ";
		
		for (vector<BlobDetector::Blob>::iterator it = blobs.begin();
			it != blobs.end(); ++it)
		{
			cout << it->color << " (" << it->size << ") ";
		}
		
		cout << endl;
		
		map<BlobDetector::BlobCategory, BlobDetector::Blob> cat =
				bd.categorize(blobs);
		cout << "Categorized " << cat.size() << " blobs" << endl;
		
		for (map<BlobDetector::BlobCategory, BlobDetector::Blob>::const_iterator it
			= cat.begin(); it != cat.end(); ++it)
		{
			cout << "    Category: " << it->first << ", Color: " << it->second.color
				<< ", Size: " << it->second.size << ", Center: ("
				<< it->second.center.x << ", " << it->second.center.y << ")"
				<< endl;
		}
		
		bd.showDebug(cat);
		cvWaitKey(100);
	}
	
	cvDestroyAllWindows();
}*/
